#include <iostream>
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <formation_avoidance/formation_structure.h>
#include "trajectory_formation/backward.hpp"


namespace backward {
backward::SignalHandling sh;
}



int main()
{

	formation_avoidance::Formation_Structure formation_test;


	formation_test.formation_structure_init(formation_avoidance::Diamond,sqrt(5)/4.0,10,1);

	std::vector<Eigen::Vector3d> pos;
	Eigen::Vector3d tmp0(100,0,0);

	Eigen::Vector3d tmp1(101,102,0);

	pos=formation_test.calculateAllPosition(tmp0,tmp1);

	for(int i=0;i<formation_test.formation_agent_num;i++)
	{
		std::cout<<pos[i](0)<<", "<<pos[i](1)<<", "<<pos[i](2)<<", "<<std::endl;
	}

	std::vector<Eigen::Vector3i> offset=formation_test.calculateCentreOffset();

	for(int id=0;id<4;id++)
	{
		ROS_INFO("offset: %d %d %d", offset[id](0),offset[id](1),offset[id](2));
	}

}